#include <Arduino.h>
#include <U8glib.h>
#include <TaskScheduler.h>
#define _TASK_SLEEP_ON_IDLE_RUN

#define armPin 2
#define beeperPin 8
#define ledPin LED_BUILTIN

U8GLIB_SSD1306_128X64 u8g(U8G_I2C_OPT_DEV_0|U8G_I2C_OPT_NO_ACK|U8G_I2C_OPT_FAST);  // Fast I2C / TWI 
char minuteBuffer[4];
char secondBuffer[4];
volatile byte timerStatus; // 0 = STOP, 1 = RUNNING, 2 = PAUSED

void timerCallback();
void minuteBeeperCallback();
void fiveMinuteBeeperCallback();
void startBeeperCallback();
void stopBeeperCallback();

Task timerTask(100, TASK_FOREVER, &timerCallback);
Task startBeeperTask(400, 2, &startBeeperCallback);
Task stopBeeperTask(400, 2, &startBeeperCallback);
Task minuteBeeperTask(300, TASK_FOREVER, &minuteBeeperCallback);
Task fiveMinuteBeeperTask(1500, TASK_FOREVER, &fiveMinuteBeeperCallback);

Scheduler runner;

byte minute;
byte second;

//bool isBeeping;

void setup() {
  u8g.begin();
  pinMode(ledPin, OUTPUT);
  
  minute = 0;
  second = -1;
  Serial.begin(115200);
  Serial.println("Scheduler TEST");
  
  runner.init();
  Serial.println("Initialized scheduler");
  
  runner.addTask(timerTask);
  runner.addTask(fiveMinuteBeeperTask);
  runner.addTask(minuteBeeperTask);
  runner.addTask(startBeeperTask);
  runner.addTask(stopBeeperTask);
  Serial.println("added timerTask");

//  isBeeping = false;
  digitalWrite(ledPin, HIGH);
    
  //timerTask.enable();
  arm();
  //armChange();
  //attachInterrupt(digitalPinToInterrupt(armPin), armChange, CHANGE);
}

void loop() {
  // put your main code here, to run repeatedly:
  runner.execute();
}

void timerCallback(){
  second++;
  if(second >= 60){
    minute++;
    second = 0;

    fiveMinuteBeeperTask.setIterations(minute / 5 + 1);
    minuteBeeperTask.setIterations(minute % 5);

    //minuteBeeperTask.delay(fiveMinuteBeeperTask.getInterval() * fiveMinuteBeeperTask.getIterations());

//    if(minute >= 5)
//      fiveMinuteBeeperTask.enable();
//    else if (minute > 0)
//      minuteBeeperTask.enable();

    fiveMinuteBeeperTask.enable();
  }

  //outputTimeToOled();
  
  Serial.print(minute);
  Serial.print(":");
  Serial.println(second);
  Serial.println(millis());
}

void minuteBeeperCallback(){  
//  if(isBeeping){
//    digitalWrite(ledPin, HIGH);
//    isBeeping = false;    
//    //noTone(beeperPin);
//    Serial.println("minute beep");
//  } else {
//    digitalWrite(ledPin, LOW);
//    isBeeping = true;
//    //tone(beeperPin, 2093);
//  }  

  tone(beeperPin, 2093, 200);
  
  if(minuteBeeperTask.isLastIteration()){
    minuteBeeperTask.disable();
  }
}

void fiveMinuteBeeperCallback(){
//  if(isBeeping){
//    digitalWrite(ledPin, HIGH);
//    isBeeping = false;
//    //noTone(beeperPin);
//    Serial.println("5 minute beep");
//  } else {
//    digitalWrite(ledPin, LOW);
//    isBeeping = true;
//    //tone(beeperPin, 2093);
//  }    

  if(fiveMinuteBeeperTask.isLastIteration()){
    minuteBeeperTask.enable();
    fiveMinuteBeeperTask.disable();
    return;
  }
  
  tone(beeperPin, 2093, 1000);
  Serial.println("5 minute beep");
}

void startBeeperCallback(){
  if(startBeeperTask.isFirstIteration()){
    tone(beeperPin, 1976, 400);
  } else if(startBeeperTask.isLastIteration()){
    tone(beeperPin, 2349, 600);
  }
}

void stopBeeperCallback(){
  if(stopBeeperTask.isFirstIteration()){
    tone(beeperPin, 2349, 400);
  } else if(stopBeeperTask.isLastIteration()){
    tone(beeperPin, 1976, 400);
  }
}

void outputTimeToOled(){

  sprintf (minuteBuffer, "%01d", minute);
  sprintf (secondBuffer, "%02d", second);
  
  u8g.firstPage();  
  do {
    // graphic commands to redraw the complete screen should be placed here          
    if(minute < 10){
      u8g.setFont(u8g_font_fur49n);
      u8g.setFontPosTop();
      u8g.drawStr(18, 6, minuteBuffer);   
    
      u8g.setFont(u8g_font_fur25n);
      u8g.setFontPosTop();
      u8g.drawStr(68, 24, ":");
          
      u8g.drawStr(84, 28, secondBuffer);
    } else {
      u8g.setFont(u8g_font_fur49n);
      u8g.setFontPosTop();
      u8g.drawStr(0, 6, minuteBuffer);   
    
      u8g.setFont(u8g_font_fur25n);
      u8g.setFontPosTop();
      u8g.drawStr(78, 24, ":");
          
      u8g.drawStr(88, 28, secondBuffer);
    }
            
    u8g.setFont(u8g_font_7x14);
    u8g.setFontPosTop();
    switch (timerStatus) {
    case 0:   // STOP
      u8g.drawStr(100, 0, "Stop");
      break;
    case 1:   // RUNNING
      u8g.drawStr(79, 0, "Running");
      break;
    case 2:   // PAUSED
      u8g.drawStr(86, 0, "Paused");
      //u8g.drawBox(0, 60, pausedMillis * 128 / pauseTimeout, 4);
      break;
    }
  } while( u8g.nextPage() );
}

void arm(){
  timerTask.enable();
  startBeeperTask.setIterations(2);
  startBeeperTask.enable();
      
  #ifdef DEBUG
    Serial.println("Arm!");
  #endif
}

void disarm(){
  timerTask.disable();
  fiveMinuteBeeperTask.disable();
  minuteBeeperTask.disable();

  minute = 0;
  second = -1;
  
  stopBeeperTask.setIterations(2);
  stopBeeperTask.enable();
  
  #ifdef DEBUG
    Serial.println("Disarm!");
  #endif
}

void armChange(){
  bool isarm = !digitalRead(armPin);    // LOW = ARM, HIGH = DISARM
  if(isarm){
    arm();
  }
  else{    
    disarm();
  }
}
